/*
 *    Copyright (c) 2014 Xiang Xu <xuxiang@mail.bnu.edu.cn>
 *
 *    Permission is hereby granted, free of charge, to any person
 *    obtaining a copy of this software and associated documentation
 *    files (the "Software"), to deal in the Software without
 *    restriction, including without limitation the rights to use,
 *    copy, modify, merge, publish, distribute, sublicense, and/or sell
 *    copies of the Software, and to permit persons to whom the
 *    Software is furnished to do so, subject to the following
 *    conditions:
 *
 *    The above copyright notice and this permission notice shall be
 *    included in all copies or substantial portions of the Software.
 *
 *    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 *    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 *    OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 *    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 *    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 *    WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 *    FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 *    OTHER DEALINGS IN THE SOFTWARE.
 */


#include "pclwraper.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/filters/statistical_outlier_removal.h"
#include "pcl/filters/radius_outlier_removal.h"
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>

PclWraper::PclWraper()
{

}

PclWraper::~PclWraper()
{

}

Status PclWraper::XYZ2PCD(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // open data file
    std::fstream ifs(file_path.c_str());

    if(ifs.is_open())
    {
        // fill in the point cloud file
        while(!ifs.eof())
        {
            pcl::PointXYZ point;
            ifs >> point.x >> point.y >> point.z;

            cloud->push_back(point);
        }

        ifs.close();

        return Status::Success();
    }

    return Status::OpenFileFail();
}

Status PclWraper::XYZL2PCD(const std::string& file_path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    // fill in the cloud data
    std::fstream ifs(file_path.c_str());

    int dummy;

    if(ifs.is_open())
    {
        // TODO: deal with the case that no comment line exits
        char first_line[2048];
        ifs.getline(first_line, 2048);  // first line is a comment line

        while(!ifs.eof())
        {
            pcl::PointXYZ point;
            ifs >> point.x >> point.y >> point.z >> dummy;

            cloud->push_back(point);
        }

        ifs.close();

        return Status::Success();
    }

    return Status::OpenFileFail();
}

Status PclWraper::PLY2PCD(const std::string& file_path, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud)
{
    pcl::PLYReader ply_reader;

    // TODO: test whether reading is success
    ply_reader.read(file_path, *cloud);

    return Status::Success();
}

Status PclWraper::SavePCD(const std::string& file_path, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud, bool
                          binary)
{
    // TODO: check return value; deal with the binary saving!
    pcl::io::savePCDFile(file_path.c_str(), *cloud);

    return Status::Success();
}

Status PclWraper::LoadPCD(const std::string& file_path, pcl::PointCloud< pcl::PointXYZ >::Ptr cloud)
{
    // TODO: check return value; deal with the binary reading!
    pcl::io::loadPCDFile(file_path, *cloud);

    return Status::Success();
}

Status PclWraper::GridSample(pcl::PointCloud< pcl::PointXYZ >::Ptr input, pcl::PointCloud < pcl::PointXYZ
                             >::Ptr output,
                             double size_x, double size_y, double size_z)
{
    // create the filtering object
    pcl::VoxelGrid<pcl::PointXYZ> grid;
    // set input
    grid.setInputCloud(input);
    // set leaf size
    grid.setLeafSize(size_x, size_y, size_z);
    // get output
    grid.filter(*output);

    return Status::Success();

}

Status PclWraper::RadiusOutlier(pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr
                                output,
                                double radius, int min_neighbors)
{
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> removal;
    removal.setInputCloud(input);
    removal.setRadiusSearch(radius);
    removal.setMinNeighborsInRadius(min_neighbors);
    removal.filter(*output);

    return Status::Success();
}

Status PclWraper::StatiscalOutlier(
    pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr output,
    int num_neighbors,	//  The number of neighbors to analyze for each point
    double stddevmul, // all points who have a distance larger than @stddevmul@ standard deviation of the mean distance
    // to the query point will be marked as outliers and removed.
    bool inliers
)
{
    // Create the filtering object
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> removal;
    removal.setInputCloud(input);
    removal.setMeanK(num_neighbors);
    removal.setStddevMulThresh(stddevmul);

    if(!inliers)
        removal.setNegative(true);

    removal.filter(*output);

    return Status::Success();
}

Status PclWraper::PassThroughFilter(pcl::PointCloud< pcl::PointXYZ >::Ptr input, pcl::PointCloud <
                                    pcl::PointXYZ >::Ptr output, const std::string& axe, double minLimits, double maxLimits, bool inside)
{
    // Create the filtering object
    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(input);
    pass.setFilterFieldName(axe);
    pass.setFilterLimits(minLimits, maxLimits);

    if(!inside)
        pass.setFilterLimitsNegative(true);

    pass.filter(*output);

    return Status::Success();
}

Status PclWraper::ExtractPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr input,		// input cloud
                               pcl::PointCloud<pcl::PointXYZ>::Ptr output,
                               double threshold, Eigen::VectorXf& plane_coeff)
{
    std::vector<int> inliers;	// plane indices in the input point cloud
    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
    PlaneModel(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(input)); // plane Model

    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(PlaneModel);	// ransac

    // set attributes...;
    ransac.setDistanceThreshold(threshold);
    ransac.computeModel();

    // get result
    ransac.getInliers(inliers);
    // copies all inliers of the model computed to another PointCloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud<pcl::PointXYZ>(*input, inliers, *plane);

    // get plane model
    ransac.getModelCoefficients(plane_coeff);

    // project the points to the plane to reduce error
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    coefficients->values.resize(4);
    coefficients->values[0] = plane_coeff(0);
    coefficients->values[1] = plane_coeff(1);
    coefficients->values[2] = plane_coeff(2);
    coefficients->values[3] = plane_coeff(3);

    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType(pcl::SACMODEL_PLANE);
    //proj.setInputCloud(plane);
    proj.setInputCloud(input);
    proj.setModelCoefficients(coefficients);
    proj.filter(*output);

    return Status::Success();
}

Status PclWraper::ComputeNormal(pcl::PointCloud<pcl::PointXYZ>::Ptr input,
                                pcl::PointCloud<pcl::Normal>::Ptr cloud_normals)
{
    // Create the normal estimation class, and pass the input dataset to it
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(input);

    // Create an empty kdtree representation, and pass it to the normal estimation object.
    // Its content will be filled inside the object, based on the given input dataset (as no other search
    // surface is given).
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ> ());
    ne.setSearchMethod(tree);

    // Use all neighbors in a sphere of radius 0.3m
    ne.setRadiusSearch(0.3);

    // Compute the features
    ne.compute(*cloud_normals);

    return Status::Success();
}





